Method of controlling position and attitude of working robot and its manipulator and apparatus thereof

ABSTRACT

By a first homogeneous transformation matrix representing a position and orientation of a moving body in an absolute space, and a second homogenous transformation matrix representing a position and attitude of a manipulator in its operating space, position and orientation information of the moving body in the absolute space, and position and attitude information of the manipulator in its operating space are represented by a same representing method, and furthermore, the position and attitude of the manipulator are controlled on the basis of information of a third homogeneous transformation matrix representing the position and attitude of the manipulator including the position and orientation information of the moving body in the absolute space obtained on the basis of the first and second homogeneous transformation matrices. Since information of the third homogenous transformation matrix includes the position and orientation information of the moving body in the absolute space, the manipulator can be controlled at a desired position and attitude in the absolute space irrespective of a condition of traveling on standstill of the moving body, thus it is possible to control the position and attitude of the manipulator for operation while running the moving body.

This application is a division of Ser. No. 787,304 filed Oct. 29, 1991now U.S. Pat No. 5,347,616.

BACKGROUND OF THE INVENTION

1. Field of the Invention

The present invention relates to a method of controlling a position andattitude of a manipulator which is mounted on a moving body and conductsvarious operations and an apparatus thereof. More particularly, itrelates to a working robot consisted of the moving body and themanipulator.

2. Description of the Related Art

In order to realize improvement of productivity and securing safety in afactory, various working robots such as a traveling robot and the like,in which an articulated manipulator is mounted on a truck which is amoving body traveling autonomously is in practical use. Such a travelingrobot is so designed that it travels autonomously to a workingdestination and stops by controlling traveling of the truck, andthereafter, the position arid attitude of the manipulator are controlledso as to conduct predetermined works.

In the traveling robot, conventionally it was difficult to taketraveling information of the truck into the position and attitudecontrol of the manipulator, so that the traveling control of the truckand position and attitude control of the manipulator were executedseparately. Specifically, for example, stop positions of the truck aredetermined in advance responsive to the working positions, and positionand attitude control information of the manipulator in a working spacerestricted by the truck stop position are decided in advance. Thus, whenthe truck stops at either of the stop positions, the position andattitude of the manipulator are controlled in response to the positionand attitude control information restricted by the stop position toexecute the predetermined work.

In the conventional traveling robot as aforementioned, however, sincethe traveling control of the truck and the position and attitude controlof the manipulator are executed separately as stated above, it isimpossible to control the manipulator to assume an attitude inpreparation for the next work while the truck travels. However,information on position and orientation of the truck while traveling arenever taken into the position and attitude control of the manipulator.Therefore, in a case where complicated work is to be carried outactually in the working position, after the truck is stopped at thepredetermined stop position, the position and attitude of themanipulator are controlled responsive to the stop position to executethe predetermined work. Thus, it is actually impossible to work themanipulator while the truck is traveling, resulting in low workingefficiency.

SUMMARY OF THE INVENTION

The present invention has been devised in view of such circumstances,therefore, it is a primary object thereof to provide a method ofcontrolling a position and attitude of a working robot and itsmanipulator and an apparatus thereof, whereby it is possible to work bycontrolling the position and attitude of the manipulator while a truckis traveling to improve the working efficiency.

The method of controlling the position and attitude of the manipulatoraccording to the present invention is the method of controlling theposition and attitude of the manipulator which is mounted on a movingbody and conducts various operations, and comprises: a step forobtaining a first homogeneous transformation matrix representing aposition and orientation of the moving body in an absolute space; a stepfor obtaining a second homogeneous transformation matrix representing aposition and attitude of the manipulator in its operating space; a stepfor obtaining a third homogeneous transformation matrix representing aposition and attitude of the manipulator including position andorientation information of the moving body in the absolute space on thebasis of the first and second homogeneous transformation matrices; and astep for controlling a position and attitude of the manipulator on thebasis of information of the third homogeneous transformation matrix.

A position and attitude controlling apparatus of the manipulatoraccording to the present invention is mounted on the moving body, forcontrolling the position and attitude of the manipulator which conductsvarious operations, and includes first matrix means for obtaining thefirst homogeneous transformation matrix representing the position andorientation of the moving body in the absolute space; second matrixmeans for obtaining the second homogeneous transformation matrixrepresenting the position and attitude of the manipulator in itsoperating space; third matrix means for obtaining the third homogeneoustransformation matrix representing the position and attitude of themanipulator including the position and orientation information of themoving body in the absolute space, in response to the homogeneoustransformation matrices obtained by the first and second means; aridcontrolling means for controlling the position and attitude of themanipulator on the basis of information of the homogeneoustransformation matrix obtained by the third matrix means.

A working robot of the present invention comprises: a moving body whichtravels autonomously; a manipulator which is mounted on the moving bodyand conducts various operations and a control device: having, in orderto control the position and attitude of the manipulator, first matrixmeans for obtaining the first homogeneous transformation matrixrepresenting the position and orientation of the moving body in theabsolute space; second matrix means for obtaining the second homogeneoustransformation matrix representing the position and attitude of themanipulator in its operating space; third matrix means for obtaining thethird homogeneous transformation matrix representing the position andattitude of the manipulator including the position and orientationinformation of the moving body in the absolute space, on the basis ofthe homogeneous transformation matrices obtained by the first and secondmeans; and controlling means for controlling the position and attitudeof the manipulator on the basis of information of the homogeneoustransformation matrix obtained by the third matrix means.

Here, the absolute space is the space represented by an absolutecoordinate system which is set independently of the moving body andmanipulator. The homogeneous transformation matrix is a 4×4 matrix whichrepresents a position, orientation and attitude of the other coordinatesystem origin observed from one coordinate system origin between the twocoordinate system origins.

In the present invention, by obtaining the first homogeneoustransformation matrix representing the position and orientation of themoving body in the absolute space, and the second homogeneoustransformation matrix representing the position and attitude of themanipulator in its operating space, position and orientation informationof the moving body in the absolute space and position and attitudeinformation of the manipulator in its operating space can be obtained bya same representation method. On the basis of the first and secondhomogeneous transformation matrices, the third homogeneoustransformation matrix representing the position and attitude of themanipulator including the position and orientation information of themoving body in the absolute space is obtained, and on the basis of thethird homogeneous transformation matrix, the position and attitude ofthe manipulator are controlled. Since information of the thirdhomogeneous transformation matrix includes the position and orientationinformation of the moving body in the absolute space, irrespective oftraveling or standstill condition of the moving body, the position andattitude of the manipulator are controlled to be desired ones.

The above and further objects and features of the invention will morefully be apparent from the following detailed description withaccompanying drawings.

BRIEF DESCRIPTION OF THE DRAWINGS

FIG. 1 is a perspective view of a working robot used for embodying amethod of controlling position and attitude of a manipulator accordingto the present invention,

FIG. 2 is a schematic block diagram of a control system for controllingthe position and attitude of a manipulator,

FIG. 3 is a perspective view showing a configuration of a travelingrobot used in a simulation experiment,

FIG. 4 is a plan view of a traveling robot used in a simulationexperiment,

FIG. 5 is a plan view of the neighborhood of a traveling robot showingexperiment results of a first experiment, and

FIG. 6 is a plan view of the neighborhood of a traveling path of atraveling robot showing experiment results of a second experiment.

DESCRIPTION OF THE PREFERRED EMBODIMENTS

In the following, the present invention will be specifically describedwith reference to the drawings showing the embodiments.

FIG. 1 is a perspective view of a traveling robot into which a positionand attitude control method of a manipulator according to the presentinvention is embodied.

In the figure, reference numeral 1 designates a truck which is a movingbody traveling autonomously. The truck 1 has its box-type truck body 10supported by a pair of right and left driving wheels 11, 11 (only rightdriving wheel is shown) and casters 12, 12, 12, 12 (only two are shown)disposed in front, rear, right and left. The left driving wheel 11 andthe right driving wheel 11 are driven independently by separate motors(not shown). On the left driving wheel 11 and the right driving wheel11, proceeding speed detectors (not shown) consisting of pulsegenerators which oscillate a pulse signal responsive to theirrevolutions are disposed respectively.

On the upper portion of the truck body 10 of the truck 1, an articulatedmanipulator 2 having a hand effectuator 20 on its apex is provided. Oreach knuckle of the manipulator 2, an angle detector (not shown) fordetecting the knuckle angle is disposed.

In the figure, a symbol Σ_(s) indicates an absolute coordinate systemset at any point (e.g. any point on the floor surface) independently ofthe truck 1 and the manipulator 2, a symbol Σ_(v) indicates a movingcoordinate system set on the truck 1, and a symbol Σ_(w) indicates aworking coordinate system set on a piece 200 to be worked forrepresenting the position and attitude of the piece 200 to be worked.

First, a method of representing the position and orientation of atraveling robot constructed as stated above in matrices will bedescribed. Here, it is assumed that the traveling robot is running in atwo-dimensional plane.

A velocity r of the truck 1 base on the absolute coordinate system Σ_(s)shown in FIG. 1 is represented as the following Equation (1) by avelocity x in an x-axis direction, a velocity y in a y-axis directionand a rotating angular velocity θ.

    r=[x, y, θ].sup.T                                    (1).

Also, proceeding velocity V_(L), V_(R) of the left driving wheel 11 andthe right driving wheel 11 are represented by a transposed matrix as thefollowing Equation (2).

    V=[V.sub.L, V.sub.R ].sup.T                                (2).

From Equations (1) and (2), the velocity r of the truck 1 is representedby using a matrix as the following Equation (3).

    r=J.sub.v (θ)V                                       (3).

Where, a matrix J_(v) (θ) in Equation (3) is represented by a matrix asshown in the following Equation (4). In addition, T represents tread(distance between left and right wheels) in Equation (4) and followingequations. ##EQU1##

A symbol θ in Equation (3) indicates an angle when the y-axis directionof the absolute coordinate system Σ_(s) is 0 and a counterclockwiserotating direction is positive.

Next, a position and orientation of the truck 1 of the traveling robotin the absolute coordinate system Σ_(s) s is obtained.

The position and orientation r^(i) of the truck i of the traveling robotis obtained as the following Equation (5), by integrating both sides ofEquation (3) with the position and orientation r^(i-1) at time (i-1)τ asan initial value, when its control period is τ. ##EQU2##

Here, v and θ in Equation (5) are obtained as the following Equations(6) and (7). ##EQU3##

A velocity v^(i) and rotating angular velocity ω^(i) of the truck 1 attime iτ can be obtained by using the aforesaid Equations (6) and (7) onthe basis of a value obtained by a sample-holding the respectiveproceeding velocities of the left driving wheel 11 and the right drivingwheel 11 for τ seconds from the time (i-1) τ.

v and θ in an integral term of Equation (5) are arbitrary time functionsdecided as the truck 1 travels, so that they can not be integratedanalytically. Therefore, the integral term of Equation (5) is obtainedby a geometric approximate calculation by operational equations as shownin the following Equations (8) to (10), using a geometric approximatecalculating method as shown in "A method of measuring the PresentPosition and Proceeding Orientation of a Moving Body" by Tsumura et al,PP 1153-1160, Japan Machinery Association, Papers Vol. 47, No.421(1981).

    r.sup.1 =r.sup.1 +Δ.sup.1                            (8) ##EQU4##

    Δθ.sup.i =ω.sup.i τ                  (10)

Here, since Equation (8) is for obtaining the integral term of Equation(5) approximately, some errors are involved, so that in order toindicate it clearly, a hat symbol " " is added to variables in Equations(8) to (10).

Next, a preparatory calculation method for taking position andorientation information of the truck 1 obtained by the methodaforementioned into the position and attitude control of the manipulator2 will be described.

In the case where the position and attitude of an articulated linkmechanism such as the manipulator 2 are to be represented, it is knownthat position and attitude information of one knuckle is represented bya 4×4 matrix, and by multiplying the 4×4 matrix representing positionand attitude information of respective knuckles, the position andattitude information of the knuckles are obtained. Therefore, in orderto take the position and orientation information of the truck 1 into theposition and attitude control of the manipulator 2, it is necessary torepresent the position and orientation information of the truck 1 by the4×4 matrix.

At first, When a matrix in which r^(i-1) in Equation (8) is representedby a homogeneous transformation expression is C^(i-1), its homogeneoustransformation matrix (C^(i-1)) is represented by a 4×4 matrix as shownin the following Equation (11). ##EQU5##

Similarly, when a matrix in which r^(i) in Equation (8 ) is representedby a homogeneous transformation expression is C^(i), its homogeneoustransformation matrix (C^(i)) is represented by, as shown in Equation(12), an equation obtained by multiplying C^(i-1) by a homogeneoustransformation matrix (ΔC^(i)) representing a variation of position andorientation of the truck 1.

    C.sup.i =C.sup.i-1 ·ΔC.sup.i                (12)

Here, ΔC^(i) in Equation (12) is represented by a homogeneoustransformation matrix as shown in the following Equation (13). ##EQU6##

Here, Δx^(i) in Equation (13) is represented geometrically by thefollowing Equation (14) , and Δy^(i) in Equation (13) representedgeometrically by Equation (15).

    Δx.sup.i =-V.sup.i τsin(Δθ.sup.i /2) (14)

    Δy.sup.i =v.sup.i τsin(Δθ.sup.i /2)  (15)

Thus, when an estimated value of initial position and orientation of thetruck 1 is C⁰, C^(i) can be obtained by, as shown in the followingEquation (16), multiplying C⁰ by ΔC^(P) (where, P=1 to i) which is avariation of position and orientation of the truck 1. ##EQU7##

The position and orientation of the truck 1 on the absolute coordinatesystem Σ_(s) is represented by a transformation matrix from the movingcoordinate system Σ_(v) to the absolute coordinate system Σ_(s). Thatis, the transformation matrix ^(S) A^(i) _(v) aforementioned representsa position and orientation of the moving coordinate system Σ_(v) viewedfrom the absolute coordinate system Σ_(s). Since there is a cumulativeerror δ_(C) ^(i) caused by sample-holding of the velocity V and rotatingangular velocity ω of the truck 1 and effects of the running roadcondition of the truck 1 in an estimated value of the position andorientation of the truck 1, the transformation matrix ^(s) A^(i) _(v) isrepresented as a product of C^(i) and the cumulative error δC^(i), asshown in the following Equation (17).

    .sup.S A.sup.i.sub.V =C.sup.i δC.sup.i               (17)

The cumulative error δC^(i) can be obtained from the following Equation(18) as a product of the transformation matrix ^(s) A^(i) _(v) and aninverse matrix of C^(i), only when the transformation matrix ^(s) A^(i)_(v) is observed.

    δC.sup.1 (C.sup.i).sup.-1.sup.s A.sup.1.sub.v        (18)

In the case where a traveling distance of the truck 1 is short to theextent that the cumulative error δC^(i) can be assumed to beinfinitesimal, δC^(i) can be approximated with a unit matrix I. Thus, insuch a case, as shown by the following Equation (19), the transformationmatrix ^(s) A^(i) _(v) and ΔC^(i) may be represented as substantiallyequal.

    .sup.S A.sub.v.sup.i ≈C.sup.i                      (19)

Next, a position and attitude control method of the manipulator 2 willbe described. When attitude vectors of the hand effectuator 20 of themanipulator 2 are vector n, vector o and vector a ε R³ (here, R³ isthree-element of real number), and a position vector is p εR³, ahomogeneous transformation matrix H representing the position andattitude of the hand effectuator 20 becomes a matrix as shown in thefollowing Equation (20). ##EQU8##

A homogeneous transformation matrix ^(v) H^(i) representing the positionand attitude of the hand effectuator 20 at time iτ it representedrelative to the moving coordinate system Σ_(v) is, when a knuckle anglevector of respective knuckles of the manipulator 2 is q^(i) εR^(n)(q^(i) is an n-element vector of real number), represented as a functionof knuckle angle vector q^(i) as shown in the following Equation (21)from a forward kinematics of the manipulator 2.

    .sup.v H.sup.i =F(q.sup.i)                                 (21)

When a transformation matrix of the hand effectuator 20 front theabsolute coordinate system Σ_(s) to the working coordinate system Σ_(w)is ^(w) A_(s), a homogeneous transformation matrix ^(w) H^(i)representing the position and attitude of the hand effectuator 20 attime iτ represented relative to the working coordinate system Σ_(w) isrepresented as a product of the transformation matrix ^(w) A_(s),aforesaid C^(i) and the homogeneous transformation matrix ^(v) H^(i) asshown in the following Equation (22). ##EQU9##

An error vector ^(w) θ^(i) representing an error from an instructionvalue of the position and attitude of the hand effectuator 20 at timeiτ, represented relative to the working coordinate system Σ_(w) isrepresented by a matrix as shown in the following Equation (23).##EQU10##

The instruction value of the new position and attitude obtained bytaking into consideration of the error vector ^(w) θ^(i) thusrepresented is obtained by the following Equation (24). Here, Δ^(w)H^(i) in Equation (24) is a matrix as shown in Equation (25). ##EQU11##

The instruction value of the position and attitude obtained by Equation(24) can be represented by an instruction value of the position andattitude based on the moving coordinate system Σ_(v) as shown in thefollowing Equation (27), by using a transformation matrix as shown inthe following Equation (26).

    .sup.v A.sub.w.sup.i =(C.sup.i).sup.-1 s A.sub.w           (26)

    .sup.v H.sub.r.sup.i =.sup.v A.sub.w.sup.i w H.sub.r.sup.i (27)

Instruction values of the knuckle angle vectors of respective knucklesof the manipulator 2 are obtained by an inverse function F⁻¹ as shown inthe following Equation (28), by a backward kinematics of the manipulator2.

    q.sub.r.sup.i+1 =F.sup.-1 (.sup.v H.sub.r.sup.i)           (28)

Next, a configuration of control system for controlling a position andattitude of the manipulator 2 as aforementioned will be described.

FIG. 2 is a schematic block diagram of a control system for controllingthe position and attitude of the manipulator 2.

In the figure, reference numeral 25 designates a manipulator dynamicsincluding a servo-system. The manipulator dynamics 25 is a block whichrepresents an actual motion with instruction value of the knucklevectors of respective knuckles of the manipulator 2 as an input, andoutputs knuckle angle vector g^(i) representing the operating state ofrespective knuckles. The knuckle angle vector q^(i) is given to aforward kinematics operation unit 26 from the manipulator dynamics 25.

In the forward kinematics operation unit 26, on the basis of the givenknuckle angle vector q^(i), a homogeneous transformation matrix ^(v)H^(i) representing the position and attitude at time iτ is obtained byusing Equation (21), and the data is given to a second coordinatetransformation unit 27.

To a position and orientation operation unit 100 for obtaining theposition and orientation of the truck 1, data of a proceeding velocityV_(L) of the left driving wheel 11, a proceeding speed V_(R) of theright driving wheel 11 and initial position, and orientation estimatedvalue (C⁰ ) of the truck 1 are given, arid in the position andorientation operation unit 100, on the basis of the data, thehomogeneous transformation matrix (C^(i)) representing the position andorientation of the truck 1 and its inverse matrix (C^(i))⁻¹ are by theusing Equation (16), and the former data is given to the secondcoordinate transformation unit 27 and the latter data is given to thefirst coordinate transformation unit 23.

In the second coordinate transformation unit 27, on the basis of thegiven homogeneous transformation matrix ^(v) H^(i) and homogeneoustransformation matrix (C^(i)), a homogeneous transformation matrix ^(w)H^(i) representing the position and attitude of the hand effectuator 20at time iτ, represented relative to the working coordinate system Σ_(w)is obtained by using Equation (22). Data of the homogeneoustransformation matrix ^(w) H^(i) obtained here is given to a positionand attitude error operation unit 21 and an instruction value operationunit 22.

To the position and attitude error operation unit 21, a homogeneoustransformation matrix ^(w) H^(i) _(d) representing the instruction valueof position and attitude control of the hand effectuator 20 is given. Inthe position and attitude error operation unit 21, on the basis of thehomogeneous transformation matrix ^(w) H^(i) _(d) representing theinstruction value and homogeneous transformation matrix ^(w) H^(i), anerror vector ^(w) e^(i) is obtained by using Equation (23). Data of theerror vector ^(w) e^(i) obtained here is given to the instruction valueoperation unit 22.

In the instruction value operation unit 22, on the basis of the givenerror vector ^(w) e^(i) and homogeneous transformation matrix ^(w)H^(i), a homogeneous transformation matrix ^(w) H^(i).sub.τ representingthe instruction value of a new position and attitude obtained by takinginto consideration of the error vector ^(w) e^(i) is obtained by usingEquations (24) and (25). Data of the homogeneous transformation matrixobtained here is given to the first coordinate transformation unit 23,

In the first coordinate transformation unit 23, on the basis of thehomogeneous transformation matrix ^(w) H^(i).sub.γ representing theinstruction value of a new position and attitude given and an inversematrix of the homogeneous transformation matrix (C^(i)), a homogeneoustransformation matrix representing the instruction value of the positionand attitude based on the moving coordinate system Σ_(v) is obtained byusing Equations (26) and (27). Data of the homogeneous transformationmatrix obtained here is given to a backward kinematics operation unit24. In the backward kinematics operation unit 24, on the basis of thegiven data, an instruction value of the knuckle angle vector ofrespective knuckles of the manipulator 2 is obtained by using Equation(28), and the instruction value of the knuckle angle vector ofrespective knuckles obtained is given to the servo-system of respectiveknuckles in the manipulator dynamics 25. Then, the manipulator 2 isdriven according to the given instruction value.

Next, results of simulation experiments on controlling a manipulatorusing the position and attitude control method of the manipulator asaforementioned will be described.

FIG. 3 is a perspective view showing a configuration of a travelingrobot used in the simulation experiment, and FIG. 4 is a plan view ofthe traveling robot. In FIG. 3 and FIG. 4, parts similar to FIG. 1 aredesignated by the same reference characters and their explanation isomitted.

The traveling robot includes a 2-link type manipulator 2 which operatesin an x-y plane with a first knuckle 2a and a second knuckle 2b asrotating centers. In this case, the homogeneous transformation matrix^(v) H^(i) representing the position and attitude of the handeffectuator 20 at time iτ, represented relative to the moving coordinatesystem Σ_(v) represented by Equation (21) is represented by a matrix asshown in the following Equation (29). ##EQU12## note, l₁ : length fromfirst knuckle to second knuckle

l₂ : length from second knuckle to edge of manipulator

q₁ ^(i) : knuckle angle of first knuckle

q₂ ^(i) : knuckle angle of second knuckle

When the working coordinate system Σ_(w) is consisted with the absolutecoordinate system Σ_(s) for simplifying the description, transformationmatrices ^(w) A_(s) and ^(s) A_(w) become a unit matrix I, so that thehomogeneous transformation matrix ^(w) H^(i) shown in Equation (22) isrepresented by a matrix as shown in the following Equation (30).##EQU13##

Since the manipulator 2 can only indicate the position in the x-y planeof z=h, a measured value was given as a target value other than (x, y)coordinates. As target positions of (x, y), a fixed point which does notmove temporally is set and an origin of the working coordinate Σ_(w) wasselected. The instructed position and attitude of the manipulator 2 isrepresented by a matrix as shown in the following Equation (31).##EQU14##

Thus, the error vector ^(w) e^(i) as shown in Equation (23) isrepresented by a matrix as shown in the following Equation (32).

    .sup.w θ.sup.i =[S.sub.x -1.sub.1 cos q.sub.1.sup.i -1.sub.2 cos (q.sub.1.sup.i +q.sub.2.sup.i), S.sub.y -vi-1.sub.1 sin q.sub.1.sup.i -1.sub.2 sin (q.sub.1.sup.i +q.sup.i.sub.2), 0, 0, 0, 0]  (32)

Then, an instruction value of the knuckle angle vector is obtained byusing Equation (28) and outputted to the servo-system of the manipulator2. It is assumed that a time constant of the first knuckle of themanipulator 2 is 0.2 second, a time constant of the second knuckle is0.1 second and a cumulative error δC^(i) of position and orientationestimated value of the truck I is a unit matrix I.

For a traveling robot having these conditions, the following first andsecond experiments were carried out.

In the first experiment, a response of the manipulator 2 was observedwhile tile traveling robot was standing still. FIG. 5 is a plan view ofthe neigborhood of the traveling robot showing the fist experimentresult, in which the operation of the manipulator 2 with time is shown.

In FIG. 5, reference character 2a designates a first knuckle of themanipulator 2, 2b designates a second knuckle of the manipulator 2 and2c designates an edge position of the manipulator 2. As is apparent fromFIG. 5, when the traveling robot is standing still, the same result asthe case where the manipulator is fixed can be obtained.

In the second experiment, a response of the manipulator 2 in the casewhere the truck 1 is driven at a velocity of 4 m/min was checked. FIG. 6is a plan view of the neighborhood of a traveling path of the travelingrobot showing the second experiment result, in which the operation ofthe manipulator 2 with the passage of time is shown.

As it is apparent from FIG. 6, it is observed that the control of themanipulator 2 with movement taken into consideration is conducted.

As such, in the position and attitude control method of the manipulatorof the present invention, since the position and attitude of themanipulator 2 is controlled during the traveling of the truck 1, in thecase where of loading and unloading baggages, for example, it can beperformed by the manipulator 2 while running the truck 1.

In this embodiment, though the traveling information of the truck 1 isdesigned to be detected by the truck 1 itself, it is not limitedthereto, in the case where the manipulator 2 of the traveling robotwherein the manipulator 2 is mounted on the truck 1 which is guided byguidance of guiding means is controlled, traveling information may beobtained from the guiding means.

As described particularly hereinabove, in the position and attitudecontrol method and its apparatus of the working robot and itsmanipulator according to the present invention, by a first homogeneoustransformation matrix representing the position and orientation of amoving body in an absolute space, and a second homogenous transformationmatrix representing the position and attitude of a manipulator in itsoperating space, position and orientation information of the moving bodyin the absolute space and position and attitude information of themanipulator in its operating space are represented by the samerepresentation method, and the position and attitude of the manipulatoris controlled on the basis of information of a third homogeneoustransformation matrix, representing the position and attitude of themanipulator including the position and orientation information of themoving body in the absolute space obtained on the basis of the first andsecond homogeneous transformation matrices.

Information of the third homogeneous transformation matrix includes theposition and orientation information of the moving body in the absolutespace, and since the manipulator can be controlled at a desired positionand attitude in the absolute space irrespective of condition of themoving body, traveling or standstill, the position and attitude of themanipulator can be controlled for operations while the moving body istraveling, so that the present invention is efficacious in improving theworking efficiency.

As this invention may be embodied in several forms without departingfrom the spirit of essential characteristics thereof, the presentembodiment is therefore illustrative and not restrictive, since thescope of the invention is defined by the appended claims rather than bythe description preceding them, and all changes that fall within themetes and bounds of the claims, or equivalence of such metes and boundsthereof are therefore intended to be embraced by the claims.

What is claimed is:
 1. A method of controlling a position andorientation of a manipulator which is mounted on a moving bodycomprising an autonomously-traveling truck and which conducts variousoperations, comprising steps of:obtaining a first homogeneoustransformation matrix representing a position and orientation of saidautonomously-traveling truck in an absolute space; obtaining a secondhomogeneous transformation matrix representing a position andorientation of said manipulator in its operating space wherein saidoperating space travels along with said truck; obtaining a thirdhomogeneous transformation matrix representing a position andorientation of said manipulator derived from position and orientationinformation of said autonomously-traveling truck in an absolute space onthe basis of said first and second homogeneous transformation matrices;and controlling a position and orientation of said manipulator in anabsolute space using information of said third homogeneoustransformation matrix during a time when said autonomously-travelingtruck is moving, to obtain a target orbit of said manipulator inpreparation for work while said truck is traveling.
 2. A controllingapparatus for a position and orientation of a manipulator which ismounted on a moving body comprising an autonomously-traveling truck andwhich conducts various operations, comprising:first matrix means forobtaining a first homogeneous transformation matrix representing aposition and orientation of said autonomously-traveling truck in anabsolute space; second matrix means for obtaining a second homogeneoustransformation matrix representing a position and orientation of saidmanipulator in its operating space wherein said operating space travelsalong with said truck; third matrix means for obtaining a thirdhomogeneous transformation matrix representing a position andorientation of said manipulator derived from position and orientationinformation of said autonomously-traveling truck in an absolute space onthe basis of said first and second homogeneous transformation matricesobtained by said first and second matrix means; and controlling meansfor controlling a position and orientation of said manipulator in anabsolute space using information of said third homogeneoustransformation matrix obtained by said third matrix means during a timewhen said autonomously-traveling truck is moving, to achieve a targetorbit of said manipulator in preparation for work while said truck istraveling.
 3. A working robot, comprising:a moving body comprising anautonomously-traveling truck; a manipulator which is mounted on saidautonomously-traveling truck and which conducts various operations; acontrolling apparatus for controlling a position and orientation of saidmanipulator, having: first matrix means for obtaining a firsthomogeneous transformation matrix representing a position andorientation of said autonomously-traveling truck in an absolute space;second matrix means for obtaining a second homogeneous transformationmatrix representing a position and orientation of said manipulator inits operating space wherein said operating space travels along with saidtruck; third matrix means for obtaining a third homogeneoustransformation matrix representing a position and orientation of saidmanipulator, derived from position and orientation information of theautonomously-traveling truck in an absolute space on the basis of saidfirst and second homogeneous transformation matrices obtained by saidfirst and second matrix means; and controlling means for controlling aposition and orientation of said manipulator in an absolute space usinginformation of said third homogeneous transformation matrix obtained bysaid third matrix means during a time when said autonomously-travelingtruck is moving, to achieve a target orbit of said manipulator inpreparation for work while said truck is traveling.